// SEE END OF FILE FOR LICENSE TERMS

#include "defines.h"
#include "dualStepMixer_defines.h"
#include <dsp.h>

#define SERVO_CALC_MAX_RANGE	2048
#define timertoQ14(n)   ((int)(n * (RMAX/ SERVO_CALC_MAX_RANGE) ) )		// This gives the mixer a +-4096 count range.
#define percenttoQ14(n) ((int)(n * (RMAX/100) ))						// Mixer coefficient range of +-200% with caution

// Defines for servo travel limits of +-25%
#define RMAX12_5		((int)(RMAX * 1.25))		// 1.25 	in 2.14 format
#define NEG_RMAX12_5	-((int) RMAX12_5)		// 1.25 	in 2.14 format
#define RMAX6_125		((int)(RMAX12_5 * 0.5))		// 0.6125   in 2.14 format
#define NEG_RMAX6_125	-((int) RMAX6_125)			// -0.6125  in 2.14 format

#include "dualStepMixer_setup.h"

// Array of formatted, rescaled autopilot output
fractional APControls[AP_MAX_CONTROLS];

// Array of formatted radio input values
fractional radioInput[MAX_INPUTS + 1];

// output of first stage mixer
fractional mixedControls[AP_MAX_CONTROLS];

// Where output values are built before scaling and output.
fractional temp_pwOut[MAX_OUTPUTS + 1];

void mixTable( const MIXER_SETTINGS* pmixSettings, const unsigned char tableSize, fractional* pdestination);

	unsigned char mixerNo;			// Index of mixer in mixer list
	unsigned char destIndex;		// Index of destination channel
	unsigned char sourceIndex;		// Index of source channel for hte mixer
	unsigned char sourceType;		// radio, controls or AP

	int sourceVal;
	int destVal;
	int mixVal;
	int coefficient;

	union longbbbb tempA;

const unsigned char controlMixCount = ( sizeof(controlMixers) / sizeof(MIXER_SETTINGS) );
const unsigned char channelMixCount = ( sizeof(controlMixers) / sizeof(MIXER_SETTINGS) );


void servoMix( void )
{

// Setup all destinations to trim points for safety
	for(destIndex = 0; destIndex <= MAX_OUTPUTS; destIndex++)
	{
		temp_pwOut[destIndex] = 0;
	};

	APControls[AP_ROLL_CONTROL] 	= timertoQ14(roll_control);
	APControls[AP_PITCH_CONTROL] 	= timertoQ14(pitch_control);
	APControls[AP_YAW_CONTROL] 		= timertoQ14(yaw_control);
	APControls[AP_THROTTLE_CONTROL] = timertoQ14(altitude_control);
	APControls[AP_FLAP_CONTROL] 	= 0;
	APControls[AP_SPOILER_CONTROL] 	= 0;
	APControls[AP_CAMBER_CONTROL] 	= 0;
	APControls[AP_WAGGLE_CONTROL] 	= timertoQ14(waggle);

	// Clear the temporary output
	for(destVal = 0; destVal <= MAX_OUTPUTS; destVal++)
	{
		temp_pwOut[destVal] = 0;
	}

	// Clear the mixed controls
	for(destVal = 0; destVal < controlMixCount; destVal++)
	{
		mixedControls[destVal] = 0;
	}

	// Setup radio input values depending 
	if(udb_flags._.radio_on == 1)
	{
		for(sourceIndex = 1; sourceIndex <= MAX_INPUTS; sourceIndex++)
		{
			radioInput[sourceIndex] = udb_pwIn[sourceIndex] - udb_pwTrim[sourceIndex];
 			radioInput[sourceIndex] = timertoQ14(radioInput[sourceIndex]);
		}
	}
	else
	{
		for(sourceIndex = 1; sourceIndex < MAX_INPUTS; sourceIndex++)
		{
			radioInput[sourceIndex] = 0;
		}
	}

	mixTable(controlMixers, controlMixCount, mixedControls);
	mixTable(surfaceMixers, channelMixCount, temp_pwOut);

	for(destIndex = 1; destIndex <= MAX_OUTPUTS; destIndex++)
	{
		destVal = temp_pwOut[destIndex];
		tempA.WW =  __builtin_mulss( destVal, SERVO_CALC_MAX_RANGE );
		destVal = tempA._.W1 << 2;

		destVal += udb_pwTrim[outputTrimChannels[destIndex]];
		destVal = udb_servo_pulsesat(destVal);
		temp_pwOut[destIndex] = destVal;
	}

	memcpy(&udb_pwOut, &temp_pwOut, sizeof(udb_pwOut));
};



void mixTable( const MIXER_SETTINGS* pmixSettings, const unsigned char tableSize, int* pdestination)
{
	MIXER_SETTINGS* pmix;

	for(mixerNo = 0; mixerNo < tableSize; mixerNo++)
	{
		pmix 			= &pmixSettings[mixerNo];
		sourceIndex 	= pmix->sourceIndex;
		destIndex 		= pmix->destIndex;
				
		switch(pmix->source)
		{
		case MIX_SRC_AP:
			sourceVal = APControls[sourceIndex];
			break;
		case MIX_SRC_CONTROLS:
			sourceVal = mixedControls[sourceIndex];
			break;
		case MIX_SRC_RADIO:
			sourceVal = radioInput[sourceIndex];
			break;
		default:
			sourceVal = 0;
			break;
		}

		if(sourceVal >= 0)
		{
			coefficient	= pmix->posCoeff;
		}
		else
		{
			coefficient	= pmix->negCoeff;
		}

		// in 2.14 format a multiply divides by 2
		// With CORCON as fractional the multiply does << 1
		tempA.WW = __builtin_mulss( coefficient, sourceVal );
		tempA.WW <<= 2;
		destVal = tempA._.W1;

		destVal += pdestination[destIndex];

		// put 25% limit on values to avoid saturation
		if( destVal > RMAX12_5 )
		{
			destVal = RMAX12_5;
		}
		else if( destVal < NEG_RMAX12_5)
		{
			destVal = NEG_RMAX12_5;
		}
		
		pdestination[destIndex] = destVal;
		
	}

};

/****************************************************************************/
// This is part of the servo and radio interface software
//
// ServoInterface source code
//	http://code.google.com/p/rc-servo-interface
//
// Copyright 2010 ServoInterface Team
// See the AUTHORS.TXT file for a list of authors of ServoInterface.
//
// ServoInterface is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// ServoInterface is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License 
// along with ServoInterface.  If not, see <http://www.gnu.org/licenses/>.
//
// Many parts of ServoInterface use either modified or unmodified code
// from the MatrixPilot pilot project.
// The project also contains code for modifying MatrixPilot to operate
// with ServoInterface.
// For details, credits and licenses of MatrixPilot see the AUTHORS.TXT file.
// or see this website: http://code.google.com/p/gentlenav
/****************************************************************************/

